/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.nurdrobotics;

import edu.wpi.first.wpilibj.*;

/**
 *  This class is used to provide a dashboard display on the DriverStation
 *  that shows the status of the major subsystems
 */
public class Dashboard {

    // declare a constant for how frequently the dashboard should update
    // The number is specified in seconds
    private static final double DASHBOARD_UPDATE_INTERVAL = 0.5;

    // declare variables to hold the messages for each dashboard item
    // This is done this way to minimize the number of times we are
    // printing the messages.
    // Printing the message every time it updates could be time consuming,
    // so instead we save off the last messsage for each status, and then
    // just print it occasionally.
    private static String m_driveStatus = "";
    private static String m_lifterStatus = "";
    private static String m_stagerStatus = "";
    private static String m_shooterStatus = "";
    private static String m_wedgeStatus = "";
    private static String m_ballCounterStatus = "";
    private static String m_errorStatus = "";
    private static String m_modeStatus = "";
    private static String m_shooterSpeed = "";

    // delcare a timer that will be used in teleopUpdate to occasionally
    // update the dashboard status
    private static Timer timer = null;

    /*
     * This routine returns a string that is exactly 3 characters long padded
     * with spaces on the right
     */
    private static String getPaddedStatus(String status) {
        /*
         * In order to overwrite any prior string that was in the location,
         * pad the message with an extra 3 spaces, and then take the first 3
         * characters of the resulting string.
         * For example, if msg = "OK", then after padding with 3 spaces and
         * taking the first 3 characters, you get "OK" followed by 1 space.
         */
        String msg  = status + "   ";
        return msg.substring(0, 3);
    }

    /*
     * Set the status of the drivetrain. Note status will be truncated
     * to three characters
     */
    public static void setDriveStatus(String status) {
        m_driveStatus = getPaddedStatus(status);
    }

    /*
     * Set the status of the lifter. Note status will be truncated
     * to three characters
     */
    public static void setLifterStatus(String status) {
        m_lifterStatus = getPaddedStatus(status);
    }

    /*
     * Set the status of the stager. Note status will be truncated
     * to three characters
     */
    public static void setStagerStatus(String status) {
        m_stagerStatus = getPaddedStatus(status);
    }

    /*
     * Set the status of the shooter. Note status will be truncated
     * to three characters
     */
    public static void setShooterStatus(String status) {
        m_shooterStatus = getPaddedStatus(status);
    }

    /*
     * Set the status of the wedge. Note status will be truncated
     * to three characters
     */
    public static void setWedgeStatus(String status) {
        m_wedgeStatus = getPaddedStatus(status);
    }

    /*
     * Set the status of the ball counter. Note status will be truncated
     * to three characters
     */
    public static void setBallCounterStatus(String status) {
        m_ballCounterStatus = getPaddedStatus(status);
    }

    /*
     * Set the mode status. Note status will be truncated
     * to three characters
     */
    public static void setModeStatus(String status) {
        m_modeStatus = getPaddedStatus(status);
    }
    
    /*
     * Set the shooter speed status. Note status will be
     * truncated to three characters
     */
    public static void setShooterSpeedStatus(String status) {
        m_shooterSpeed = getPaddedStatus(status);
    }

    /*
     * Set the status of the error message. This message is
     * not truncated
     */
    public static void setErrorStatus(String status) {
        m_errorStatus = status;
    }

    /*
     * Perform an update and send it to the DriverStationLCD
     */
    public static void update()
    {
        // get the instance of the DriverStation LCD
        DriverStationLCD lcd = DriverStationLCD.getInstance();

        // the maximum line length is 21, which can be obtained as
        // DriverStationLCD.kLineLength. Print two columns of status
        // over three rows
        lcd.println(DriverStationLCD.Line.kUser2, 1, "DRIVE: " + m_driveStatus);
        lcd.println(DriverStationLCD.Line.kUser2, 12, "WEDGE: " + m_wedgeStatus);
        lcd.println(DriverStationLCD.Line.kUser3, 1, " LIFT: " + m_lifterStatus);
        lcd.println(DriverStationLCD.Line.kUser3, 12, "STAGE: " + m_stagerStatus);
        lcd.println(DriverStationLCD.Line.kUser4, 1, "SHOOT: " + m_shooterStatus);
        lcd.println(DriverStationLCD.Line.kUser4, 12, "BALLS: " + m_ballCounterStatus);
        lcd.println(DriverStationLCD.Line.kUser5, 1, "ERR: " + m_errorStatus);
        lcd.println(DriverStationLCD.Line.kUser5, 12, "SPEED: " + m_shooterSpeed);
        lcd.println(DriverStationLCD.Line.kMain6, 1, "MODE: " + m_modeStatus);

        // send all the messages to the Driverstation LCD
        lcd.updateLCD();
    }

    /**
     * This function is called continuously while in teleoperated mode
     */
    public static void teleopUpdate() {
        // check to see if the timer needs to be created
        if(timer == null) {
            // create the timer and start it
            timer = new Timer();
            timer.reset();
            timer.start();
        }

        // if the timer has not expired, return
        if(timer.get() < DASHBOARD_UPDATE_INTERVAL) {
            return;
        }

        // perform the update
        update();

        // stop the timer, reset it, and start it again
        timer.stop();
        timer.reset();
        timer.start();
    }
}
